//******卡尔曼参数************		
let Q_angle = 0.001;  
let Q_gyro = 0.003;
let R_angle = 0.5;
let dt = 0.01;	                  //dt为kalman滤波器采样时间;
let C_0 = 1;
let Q_bias = 0;
let Angle_err = 0;
let PCt_0 = 0;
let PCt_1 = 0;
let E = 0;
let K_0 = 0;
let K_1 = 0;
let t_0 = 0;
let t_1 = 0;
let Pdot =[0,0,0,0];
let PP = [[ 1, 0 ],[ 0, 1 ]];


function kalman_filter(angle,accel_ax,gyro_y) {
  angle+=(gyro_y - Q_bias) * dt; //先验估计

	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1]=- PP[1][1];
	Pdot[2]=- PP[1][1];
	Pdot[3]=Q_gyro;
	
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = accel_ax - angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
  gyro_y   = gyro_y - Q_bias;	 //输出值(后验估计)的微分=角速度
  
  return {
    angle: angle,
    gyro_y: gyro_y
  }
}

module.exports = kalman_filter;